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Abstract — The  state  space  description  of  some  physical  sys¬ 
tems  possess  nonlinear  equality  constraints  between  some  state 
variables.  In  this  paper,  we  consider  the  problem  of  applying  a 
Kalman  filter-type  estimator  in  the  presence  of  such  constraints. 
We  categorize  previous  approaches  into  pseudo-observation  and 
projection  methods  and  identify  two  types  of  constraints — those 
that  act  on  the  entire  distribution  and  those  that  act  on  the  mean  of 
the  distribution.  We  argue  that  the  pseudo-observation  approach 
enforces  neither  type  of  constraint  and  that  the  projection  method 
enforces  the  first  type  of  constraint  only.  We  propose  a  new  method 
that  utilizes  the  projection  method  twice — once  to  constrain  the 
entire  distribution  and  once  to  constrain  the  statistics  of  the 
distribution.  We  illustrate  these  algorithms  in  a  tracking  system 
that  uses  unit  quaternions  to  encode  orientation. 

Index  Terms — Kalman  filtering,  quaternions,  measurement  ma¬ 
trix,  nonlinear  constraints. 


1.  Introduction 

SOME  physical  systems  have  equality  constraints  between 
their  state  variables.  These  constraints  can  arise  for  several 
reasons.  Some  constraints  arise  from  the  basic  laws  of  physics. 
The  mass  of  constituents  in  a  sealed  chemical  reactor,  for  ex¬ 
ample,  must  remain  constant  throughout  the  reaction  process 
[1].  Other  constraints  arise  from  the  mathematical  description 
of  a  state  vector.  If  the  state  is  modelled  as  a  rotation  matrix, 
the  rows  of  the  matrix  must  be  orthonormal  [2] .  Constraints  can 
arise  from  kinematic  or  geometric  considerations  of  a  system. 
The  coordinated  turn  model  for  an  aircraft,  for  example,  as¬ 
sumes  that  the  acceleration  vector  is  orthogonal  to  the  velocity 
vector  [3],  [4]. 

A  number  of  approaches  have  been  developed  to  apply  these 
equality  constraints  within  the  Kalman  filter  (KF)  framework. 
These  can  be  broadly  classified  into  pseudo-observation,  pro¬ 
jection  and  reparameterization  methods.  The  pseudo-observa¬ 
tion  method  creates  a  fictitious  observation  whose  variance  is 
zero  [3]-[9].  By  substitution,  it  can  be  shown  that  the  KF  up¬ 
date  equations  project  the  state  estimate  onto  the  constraint  sur- 
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face  [7].  The  projection  approach  constructs  a  projection  oper¬ 
ator  that  transforms  the  estimate  so  that  it  lies  on  the  constraint 
surface  [5],  [10]-[12].  The  reparameterization  approach  simply 
reparameterizes  the  system  so  that  the  equality  constraint  is  not 
required  [13].  Since  the  focus  of  this  work  is  on  examining  the 
effects  of  different  algorithms  for  constrained  estimation,  the 
reparameterization  approach  does  not  fall  within  the  scope  of 
this  paper.  1  Thus,  we  focus  on  the  first  two  methods. 

In  this  paper,  we  argue  that  although  the  pseudo-observation 
and  projection  approaches  have  clear,  simple,  intuitive  interpre¬ 
tations  when  the  constraints  are  linear,  none  of  these  properties 
hold  when  the  constraints  are  nonlinear.  The  main  reason  is  that 
both  approaches  implicitly  assume  that  if  the  probability  distri¬ 
bution  obeys  the  nonlinear  constraint,  then  the  mean  of  the  dis¬ 
tribution  (which  is  the  property  maintained  in  the  filter)  obeys 
the  constraint  as  well.  The  pseudo-observation  method  makes 
the  further  assumption  that  the  Kalman  filter  update  rule,  which 
is  a  linear  projection  operator,  is  sufficient  to  constrain  a  prob¬ 
ability  distribution  to  a  nonlinear  surface. 

To  overcome  these  difficulties,  we  propose  a  two-step  con¬ 
straint  application  method.  The  first  step  applies  the  projection 
method  to  the  unconstrained  estimate.  As  a  result,  the  proba¬ 
bility  distribution  of  the  estimate  is  constrained  to  lie  along  the 
constraint  surface.  This  causes  the  covariance  in  the  estimate  to 
decrease.  In  the  second  step,  the  distribution  is  translated  so  that 
its  mean  lies  on  the  constraint  surface.  This,  in  turn,  causes  the 
covariance  in  the  estimate  to  increase. 

The  structure  of  this  paper  is  as  follows.  The  problem 
statement  is  described  in  Section  II.  Section  III  introduces  an 
illustrative  example  which  is  used  throughout  much  of  this 
paper:  estimating  an  angle  of  rotation  about  a  single  axis  using 
complex  numbers.  We  analyze  the  pseudo-observation  and 
projection  methods  and  show  that  they  fail.  Section  IV  exam¬ 
ines  the  properties  of  the  constraints  in  detail  and  identifies 
two  types  of  constraints.  The  first  type  is  a  constraint  on  each 
sample  in  the  entire  distribution.  The  second  type  is  a  constraint 
on  just  the  mean  of  the  estimate.  We  show  that  neither  the 
pseudo-observation  nor  the  projection  approaches  fully  satis¬ 
fies  either  constraint  interpretation.  Section  V  derives  the  new 
two-step  approach  for  applying  constraints.  In  Section  VI,  the 
new  algorithm  is  illustrated  in  a  head  tracking  application  that 
uses  unit  quaternions  to  represent  orientation.  Conclusions  are 
drawn  in  Section  VII. 

^We  assume  that  if  a  system  has  a  nonlinear  constraint,  the  designer  has  deter¬ 
mined  that  this  is  an  appropriate  representation  to  use.  In  addition,  the  compar¬ 
ison  between  reparameterization  and  using  the  constraint  algorithms  is  likely  to 
be  highly  system  dependent. 
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Fig.  1.  Constrained  fiiter;  The  prior  estimate  is  predicted  to  give  x(fe|(fe  -  1)).  The  fiiter  is  updated  with  the  observation  to  give  the  unconstrained  estimate 
x*(fe|fc).  The  constraint  is  appiied  to  give  the  finai  estimate  x(fc|fc). 


II.  Problem  Statement 

We  seek  the  mini  mum-mean  squared  error  estimate  of  the 
state  vector  of  the  noniinear  discrete  time  system 

x(k)  =  f[x(fc  —  l),u(A;),  v(fc)] 
z(k)  =  h[x(A;),u(A;),w(A;)] 

where  x(fc  - 1)  is  the  state  of  the  system  at  time  step  k-l,  u{k) 
is  the  controi  input  vector,  v(fc)  is  the  noise  process  due  to  dis¬ 
turbances  and  modelling  errors,  z{k)  is  the  observation  vector, 
and  w{k)  i  s  measurement  noi  se.  1 1  i  s  assumed  that  the  noi  se  vec¬ 
tors  v(A:)  and  w(fc)  obey  the  usuai  zero  mean  and  i  ndependence 
assumptions. 

An  equality  constraint  exists  between  the  state  variables 
which  can  be  written  in  the  form 


c[x(A:)]  =  0.  (1) 

Furthermore,  a  projection  function  p[x(A;)]  exists  such  that 


c[p[x(A:)]]  =  0  (2) 

for  ali  values  of  x(fc). 

We  use  the  notation  that  the  estimate  at  time  step  i,  using  ali 
observations  up  to  time  step  j,  is  the  random  vector  x(7|j)  with 
mean  and  covariance  P(7|i).  We  require  the  condition 
that  the  estimate  be  consistent  [14].  I  n  other  words 


P(*li)  -  E[x(7|j)x'^(7|j)]  >  0  (3) 

where  x(7|j)  =  x{i\j)  -x(7|j)  is  the  error  in  the  estimate,  and 
>  0  means  that  the  result  is  positive  semidefinite. 

The  constrai nt  i s  to  be  appi i ed  usi ng  the  archi tectu re  show n  i n 
Fig.  1:  The  filter  is  initiaiized,  predicted,  and  updated  with  the 
measurement  to  give  an  unconstrained  estimate  ±*(k\k)  with 
covariance  P*(fc|A;).  The  constraint  is  appiied,  and  the  resulting 
estimate  {■k(k\k),P{k\k)}  obeys  the  constraint 


c[x(fc|A:)]  =  0. 

This  form  is  consistent  with  Alouani’s  suggestion:  The  con¬ 
straint  is  only  applied  to  the  updated  estimate  and  is  thus  likely 
to  be  most  accurate[4].2 

^Exactly  the  same  approach  was  subsequently  proposed  by  Simon  [8], 


In  the  special  case  where  the  constraints  are  linear,  the  con¬ 
straint  and  projection  equations  are  [5] 

c[x(fc)]  =  Cx(fc)  —  c 

p[x(fc)]  =  x(k)  —  DC'^(CDC'^)~^ 

X  (c  —  Cx(fc))  (4) 

where  C  and  c  define  the  linear  constraint  (such  that  Cx  =  c), 
and  D  is  a  positive  semidefinite  weighting  matrix  that  can  be 
chosen  to,  for  example,  minimize  |P(A;|fc)|. 

A  number  of  approaches  for  estimation  in  the  presence  of 
these  constraints  have  been  derived  and,  as  explained  in  the  in¬ 
troduction,  we  consider  the  \.\\Q  pseudo-observation  and  the  pro¬ 
jection  methods. 

A.  Pseudo-Observation  Method 

The  pseudo-observation  method  generates  a  fictitious  obser¬ 
vation  from  the  constraint  function.  The  observation  model  is 


z{k)  =  c[x(A;)]. 

The  value  observed  from  the  system  is  always  treated  as  0  with 
variance  0.  Using  the  EKF  the  update  is 

x(k\k)  =  x*{k\k)  -  W(A;)c[x*(A;|fc)]  (5) 

P(k\k)  =  P*{k\k)  -  W{k)S(k)W'^{k)  (6) 

where 

S{k)  =  VcP*(fc|A:)V^c 
W{k)  =  P*{k\k)V'^cS-^{k) 

and  Vc  is  the  j  acobian  of  c  evaluated  about  x*{k\k).^ 

This  approach  was  first  proposed  Tahk  [3],  who  consid¬ 
ered  the  problem  of  applying  linear  equality  constraints  and 
then  demonstrated  a  linearized  version  on  a  coordinated  turn 
example.  Chia  derived  the  same  result  when  he  extended  his 
batch  constrained  estimation  scheme  [15]  to  a  recursive  form  to 
estimate  the  parameters  of  electrically  stimulated  muscles  [5]. 
Doran  proposed  exactly  the  same  method  for  economic  systems 
and  showed  how  it  could  be  used  to  estimate  the  determinants 
of  exchange  rate  and  population  growth  rates  in  Australia 
[6].  Wen  showed  that  the  pseudo-measurement  approach  was 
equivalent  to  projecting  the  estimate  to  the  subspace  where  the 
constraint  was  guaranteed  to  hold  [7].  Takh,  Doran,  and  Wen 
all  argued  that  extending  the  method  to  nonlinear  systems  is 
straightforward  and  analogous  to  the  EKF:  Simply  linearize 

^T his  approach  can  only  be  used  if  S(fe)  is  invertible.  This  is  normally  ensured 
by  the  injection  of  process  noise  of  the  appropriate  structure  in  the  prediction 
step. 


2776 


all  the  nonlinear  equations.  To  offset  linearization  errors,  they 
recommend  modifying  the  constraint  equation  so  that  the  co- 
variance  on  the  pseudo-observation  is  a  non-zero  value.  A  louani 
considered  the  application  of  nonlinear  constraints  to  study  the 
effect  of  target  kinematics  [4].  To  compensate  for  linearization 
errors  and  to  account  for  the  fact  that  he  was  applying  a  motion 
heuristic,  he  introduced  an  adaptive  weighting  scheme  that 
allowed  the  noise  to  decay  slowly  through  time.  This  causes 
the  constraint  to  be  applied  progressively  more  tightly. 

However,  to  the  best  of  our  knowledge,  Geeter  was  the  first 
(and  only)  author  to  explicitly  consider  the  issue  that  nonlinear 
equality  constraints  are  fundamentally  different  from  linear 
equality  constraints.  He  argued  that  nonlinear  constraints  are 
plagued  by  two  sources  of  errors:  truncation  errors  and  base 
point  errors  [9].  Truncation  errors  arise  because  the  statistics 
of  the  distribution,  which  has  undergone  a  nonlinear  transfor¬ 
mation,  cannot,  in  general,  be  calculated  precisely.  Rather,  a 
lower  order  approximation  (such  as  a  Taylor  Series  expansion 
truncated  to  a  particular  order)  must  be  used.  Base  point  errors 
occur  because  the  filter  linearizes  around  the  estimated  value 
of  the  state  rather  than  the  true  value.  Asa  result,  the  constraint 
surface  is  not  oriented  correctly.  To  overcome  these  difficulties, 
Geeter  developed  an  algorithm  known  as  the  Smoothly  Con¬ 
strained  Kalman  Filter  (SCKF)  [13].  This  algorithm  transforms 
hard  constraints  into  soft  constraints  and  provides  an  exponen¬ 
tial  weighting  term  that  progressively  tightens  the  constraints. 

All  of  these  methods  require  the  injection  of  stabilizing 
process  noise  into  the  constraint  process,  meaning  that  the 
constraint  is  loose:  The  state  is  not  guaranteed  to  obey  the 
constraint  value.  In  some  cases,  this  is  acceptable.  In  the  ap¬ 
plications  developed  by  Takh  and  A  louani,  for  example,  they 
were  applying  target  motion  heuristics.  However,  this  approach 
is  not  appropriate  when  strict  mathematical  constraints  must  be 
applied.  In  this  situation,  the  projection  approach  can  be  used. 

B.  Projection  Approach 

The  projection  approach  applies  the  projection  function  di- 
rectl y  to  the  state  esti  mate.  F or  exampi e,  usi  ng  the  E  K  F,  the  con- 
strained  mean  and  covariance  are  given  by 

yi(k\k)  =  p[r(k\k)]  (7) 

P(k\k)  =  VpP*(fc|(fc  -  l))V^p  (8) 

where  Vp  is  the]  acobian  of  p  and  is  calculated  about  x*(fc|A;). 
From  the  definition  of  the  projection  function  in  (2),  the  con¬ 
straint  is  guaranteed  to  hold  true. 

Despite  its  simplicity,  relatively  few  authors  have  used 
this  approach.  Nihan  considered  the  problem  of  estimating 
origin-destination  matrices  for  road  networks  [11].  To  conserve 
the  number  of  vehicles,  the  rows  of  the  matrix  must  sum  to 
one.  N  ihan  constrained  the  estimate  by  dividing  each  row  by  its 
sum.  Azuma  considered  the  problem  of  estimating  the  pose  of  a 
human  head  [10].  He  used  quaternions  to  estimate  the  attitude. 
To  guarantee  the  normalization  constraint,  the  quaternion  states 
were  divided  by  their  length.  In  considering  the  problem  of 
enforcing  the  orthonormal  properties  of  rotation  matrices, 
Choukroun  proposed  a  method  known  as  optimal  brute  force 
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orthogonalization,  which  is  used  to  find  the  closest  (in  the 
Frobenius  norm  sense)  orthonormal  matrix  to  a  given  matrix 
[16].  He  showed  that  this  was  equivalent  to  the  orthogonal 
Procrustes  problem  and  that  its  closed  form  solution  can  be 
expressed  in  terms  of  a  projection  operation.  Durrant-Whyte 
considered  the  problem  of  applying  coordinate  transformations 
to  parameterized  geometric  objects  [17].  Rigid  body  transfor¬ 
mations  introduce  constraints  between  configurations  of  points 
(such  as  the  distances  between  points  do  not  change).  Recently, 
Yang  considered  the  problem  of  tracking  ground  vehicles 
[12].  Nonlinear  constraints  arise  when  the  state  estimates  are 
constrained  to  lie  on  curved  roads.  He  introduced  a  nonlinear 
projection  method  for  second  order  constraints  using  Lagrange 
multipliers. 

Although  the  pseudo-observation  and  projection  methods 
share  the  property  that  they  project  the  state  estimate  to 
the  constraint  surface,  they  are  qualitatively  different.  The 
pseudo-observation  method  uses  the  Kalman  filter’s  linear 
update  rule.  Therefore,  the  projection  operator  Is  linear  and 
its  parameters  are  chosen  to  minimize  the  mean  squared  error 
estimate.  The  projection  method  can  utilize  any  projection 
operator  that  is  consistent  with  (2).  However,  if  this  operator 
takes  no  account  of  the  covariance  matrix,  it  can  actually  cause 
the  covariance  to  Increase  [5]. 

We  now  Illustrate  these  approaches  In  a  simple  example. 

III.  Estimating  Angles 

Consider  the  problem  of  estimating  an  angle  of  rotation  about 
a  single  axis.  One  (non-mlnimal)  way  to  represent  this  rotation 
is  to  use  complex  numbers 


:x.  =  X  +  iy 

where  ||x||  =  1. 

F  i  g.  2  shows  a  set  of  unconstrai  ned  esti  mates  w  hose  mean  and 
covariances  do  not  satisfy  the  equality  constraint.  Each  estimate 
was  generated  from  calculating  the  mean  and  covariance  of  a 
M  onte  Carlo  sample  of  4  x  10®  samples  of 


X  =  COS0 -|- 7sin0  (9) 

where  0  Is  a  normally  distributed  random  vector  for  eight  dif¬ 
ferent  values  of  the  mean  (steps  of  7r/4)  and  standard  deviation 
ae  =  Tt/S.  These  unconstrained  estimates  have  several  impor¬ 
tant  properties. 

1)  Each  individual  sample  satisfies  the  nonlinear  constraint 
||x||  =  1. 

2)  The  mean  of  each  distribution  has  the  property  ||x||  <  1. 
This  is  a  direct  consequence  of  Jensen’s  Inequality  [18], 
where  the  constraint  is  convex,  and  therefore,  its  mean 
value  (for  all  non-zero  values  of  the  covariance)  must  lie 
within  the  constraint  surface. 

3)  Although  the  distribution  is  oriented  so  that  the  major 
axis  is  orthogonal  to  the  radial  line  that  passes  through  the 
mean,  the  covariance  is  not  singular.  The  reason  is  that 
although  x  and  y  are  not  independent,  the  relationship 
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Fig.  2.  U  nconstrained  estimates.  The  mean  values  lie  at +,  and  the  covariances 
[1(7  values)  are  represented  by  the  dashed  ellipse.  Theunitcircleisshown  using 
dotted  lines. 


Fig.  3.  Unconstrained  and  constrained  estimates  using  the  linearized  pseudo¬ 
observation  method.  The  unconstrained  estimates  are  the  set  of  dashed  ellipses 
with  means  at  +  .  The  constrained  estimates  are  shown  as  solid  ellipses  (col¬ 
lapsed  to  lines  because  the  covariances  are  singular)  with  means  at  o.  The  unit 
circle  is  drawn  using  a  dotted  line. 


is  nonlinear  and  is  not  fully  described  by  the  correla¬ 
tion  structure  (which  is  a  first  order  parameterization  of 
dependency). 

We  now  illustrate  the  effect  of  the  constraint  algorithms  in  this 
example. 


A.  Pseudo-Observation 

The  constraint  function  c[x]  can  be  written  as 


c[x]  =  +  j/2  -  1  =  0.  (10) 

Applying  asingleupdate  does  worlead  to  anormalized  estimate 
after  a  single  iteration.  The  reason  is  that  the  EKF  can  be  con¬ 
sidered  to  be  a  single  step  in  an  optimization  algorithm  [19]. 
Therefore,  an  iterated  EKF  [20]  was  applied  with  a  threshold  of 
10“^  on  the  change  in  the  normalized  estimate  between  succes¬ 
sive  updates.  Each  update  required  no  more  than  eight  iterations. 

The  normalized  estimates  are  shown  in  Fig.  3.  At  first  sight, 
this  result  seems  reasonable:  The  normalized  estimates  lie  on 
the  uni t  cl  rcl e,  show i  ng  that  the  esti  mate  sati sfies  the  constrai  nt. 
Furthermore,  the  effect  of  the  constraint  has  been  reflected  in 
the  covariance  matrix:  It  has  collapsed  so  that  all  of  the  un¬ 
certainty  now  lies  on  the  tangent  to  the  unit  circle.  However, 
this  result  is  not  mathematically  reasonable  because  the  uncer¬ 
tainty  is  incompatible  with  the  constraint  surface.  Consider  the 
constrained  estimate  that  lies  at  (1,0).  Because  the  variance  is 
non-zero,  there  is  some  uncertainty  in  this  estimate,  and  so,  the 
true  value  can  lie  at  a  point  where  y  7^  0.  Because  the  magni¬ 
tude  of  the  estimate  is  1,  the  magnitude  of  x  must  decrease  as 
the  magnitude  of  y  increases.  However,  the  covariance  matrix 
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Fig.  4.  Unconstraine(d  an(j  constrai ne(d  estimates  using  the  second  order  UT 
pseudo-observation  algorithm.  The  constrained  estimates  are  the  set  of  dashed 
ellipses  with  means  at +  .  The  constrained  esti  mates  are  shown  as  solid  ellipses 
with  means  at  o.  The  unit  circle  is  drawn  as  the  dotted  line. 

implies  that  the  value  of  x  is  known  perfectly,  and  therefore,  its 
value  cannot  change. 

G  eeter  proposed  that  noni  I  near  constrai  nts  are  different  i  n  two 
ways:  truncation  and  base-point  errors  [9].  To  testGeeter’sfirst 
hypothesis,  we  utilized  the  second  order  Unscented  Transfor¬ 
mation  (UT)  [21].  The  UT  implicitly  calculates  the  second  and 
hi  gher  order  terms  i  n  the  T ayl  or  seri  es  expansi  on  about  the  pri  or 
mean  and  should  thus  lead  to  a  more  accurate  estimate.  Since 
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Fig.  5.  Unconstrained  and  constrained  estimates  using  the  SCKF  algorithm. 
T  he  unconstrai  ned  esti  mates  are  the  set  of  dashed  el  I  i  pses  w  ith  means  at  + .  T  he 
constrained  estimates  are  shown  as  solid  ellipses  (collapsed  to  short  solid  lines 
in  some  cases)  with  means  at  o.  The  unit  circle  is  drawn  with  a  dotted  line. 

this  is  a  2-D  example,  a  value  of  =  1  leads  to  the  result  that 
the  mean  is  calculated  precisely,  and  the  covariance  is  calculated 
correctly  to  the  second  order.  T  he  update  was  repeatedly  appi  i ed 
until  the  magnitude  of  the  difference  between  successive  esti¬ 
mates  was  10“^.  The  results  are  shown  in  Fig.  4.  Although  the 
covariance  matrices  are  not  singular,  the  mean  values  have  not 
changed  significantly  from  their  original  unnormalized  state. 

To  test  Geeter’s  second  hypothesis,  we  applied  the  SCKF. 
Fig.  5  shows  the  results  using  Geeter’s  recommended  value 
4  threshold  =  100.  Surprisingly,  the  algorithm  fails  to  converge 
to  a  solution  when  the  nominal  angle  aligns  with  one  of  the 
coordinate  axes.4  For  the  results  that  lie  off  the  coordinate 
axes,  the  results  appear  slightly  better:  The  covariance  matrices 
are  non-singular.  However,  the  constraint  is  not  obeyed  per¬ 
fectly— the  norm  is  0.984  instead  of  1.0. 

B.  Projection 

The  projection  function  we  apply  is^ 

X  =  x7||x*||.  (11) 

Fig.  6  plots  the  results  using  linearization.  The  constraint 
is  automatically  satisfied,  and  no  iterative  scheme  is  required. 
H  owever,  once  agai  n,  the  esti  mate  covari  ance  matrix  i  s  si  ngul  ar, 
and  thus,  this  method  suffers  from  the  same  difficulties  as  the 
pseudo-observation  update.  Fig.  7  shows  the  results  when  the 
LIT  is  applied.  The  results  are  similar  to  those  in  Fig.  4:  The 

4We  believe  this  is  because  the  SCKF,  like  the  other  pseudo-observation 
methods,  actually  requires  stabilizing  noise  to  be  injected  into  state  estimate. 
The  correct  implementation  of  the  SCKF  should  interleave  a  single  iteration 
of  the  constraint  equation  between  the  normal  Kalman  filter  prediction-update 
cycles  [22]. 

^This  function,  rather  than  y  —  ^/l  -  x‘^,  was  chosen  because  both  x  and  y 
are  guaranteed  to  be  real  for  all  ||x||  >  0. 
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Fig.  6.  U  nconstrained  and  constrained  estimates  using  the  linearized  projection 
algorithm.  T  he  unconstrai  ned  esti  mates  are  the  set  of  dashed  el  I  i  pses  w  ith  means 
at  +  .  The  constrained  esti  mates  are  shown  as  solid  ellipseswith  means  ato. The 
unit  circle  is  drawn  as  the  dotted  line. 
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Fig.  7.  U  nconstrained  and  constrained  estimates  using  the  UT  projection  algo¬ 
rithm.  The  unconstrained  estimates  are  the  set  of  dashed  ellipseswith  means  at 
+  .  The  constrained  estimates  are  shown  as  solid  ellipses  with  means  at  o.  The 
unit  circle  is  drawn  with  the  dotted  line. 

constrained  mean  does  not  move  significantly  from  the  uncon¬ 
strained  mean.  However,  the  covariance  is  now  slightly  larger. 

This  example  illustrates  that  both  the  pseudo-observation 
and  projection  approaches  lead  to  undesired  behaviors:  The 
linearized  approaches  lead  to  singular  covariance  matrices;  the 
higher  order  versions  of  these  transformations  lead  to  mean 
estimates  whose  values  do  not  change;  Geeter’s  algorithm  leads 
to  an  unnormalized  estimate  or  an  estimate  with  zero  variance. 
We  believe  that  this  example  is  not  an  isolated  special  case  but 
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arises  from  a  misunderstanding  as  to  what  constraint  is  being 
satisfied. 

IV.  M  EANiNG  OF  Constraints 

We  believe  that  the  difficulties  iliustrated  in  the  previous 
section  arise  from  the  fact  that  there  are,  in  fact,  two  types  of 
constraints  that  can  be  defined.  The  first  type— Type  I  con¬ 
straints — appiy  a  constraint  to  each  point  of  the  distribution 

c[x(fc|A:)]  =  0. 

Type  II  constraints  are  constraints  on  the  estimate  propagated 
by  the  Kalman  filter.  These  are  normally  posed  as  constraints 
on  the  mean  estimate 


Fig.  8.  Effect  of  the  pseudo-observation  method  ontheunnormaiized  distribu¬ 
tion.  The  unit  circie  is  drawn  as  a  dashed  iine  and  the  M  onte  Cario  sampies  as 
points,  (a)  Overaii  view,  (b)  Detaiied  view. 


c[x(fc|A:)]  =  0. 

Type  I  constraints  provide  information  about  the  entire  shape 
of  the  probability  distribution  of  x(fc).  This  provides  additional 
information  and  reduces  the  uncertainty  in  the  distribution.  A 
Type  1 1  constraint,  however,  only  specifies  information  about  the 
expected  value  of  the  distribution  of  x(fc);  no  other  information 
is  provided.  Therefore,  aType  1 1  constraint  does  not  provide  suf¬ 
ficient  information  to  reduce  the  uncertainty  in  the  distribution. 

The  relationship  between  the  two  types  is  laid  out  in  the  fol¬ 
lowing  theorem. 

Theorem  1:  The  Type  I  constraint  subsumes  the  Type  II  con¬ 
straint  only  if  the  constraint  is  linear. 

Proof:  Assume  the  Type  I  constraint  holds.  Ignoring  the 
time  index  for  convenience,  each  sample  in  the  constrained  dis¬ 
tribution  X  obeys  the  property 

c[x]  =  0. 

Equivalently,  each  sample  can  be  obtained  from 


This  theory  has  been  demonstrated  by  the  example  in 
Section  III.  The  original  M  onte  Carlo  sample  was  drawn  so  that 
the  Type  I  constraint  was  satisfied,  but  the  Type  II  constraint 
was  not  satisfied. 

The  properties  of  the  pseudo-observation  algorithm  can  be 
assessed  in  terms  of  Type  I  and  Type  1 1  constraints. 

Theorem  2:  The  pseudo-observation  approach  applies  the 
Type  I  constraint  only  if  either  a)  all  samples  obey  the  Type  I 
constraint  already  orb)  if  the  Type  I  constraint  is  linear. 

Proof:  Consider  the  pseudo-observation  update  (5).  Ap¬ 
plying  it  to  each  sample  in  the  distribution  gives 


x(A:)  =  x(A:  —  1)  —  W(fc)c[x(fc  —  1)].  (12) 

In  the  case  where  the  Type  I  constraint  is  already  obeyed, 
c[x(fc  -  1)]  =  0;  therefore,  no  update  occurs.  If  the  Type  I 
constraint  is  not  obeyed,  the  sample  is  moved  by  a  weighted 
value  of  the  constraint  equation.  If  the  translated  sample  is  to 
obey  the  nonlinear  constraint,  it  must  now  obey  the  condition 


x  =  p[x*] 

where  X*  is  a  sample  in  the  unconstrained  distribution.  Now,  the 
Type  II  constraint  is  obeyed  if 

c[x]  =  0. 

However,  since 

x  =  E[p[x*]] 

X  obeys  the  Type  II  constraint  if  it  can  be  written  as 
x=p[r]. 

Therefore,  the  Type  I  constraint  subsumes  the  Type  1 1  constraint 
if  there  exists  a  x*  such  that 

p[x*]  =  E[p[x*]]. 

In  general,  this  can  only  be  satisfied  if  p  is  linear.  ■ 


c[x(A:  —  1)  —  W(fc)c[x(fc  —  1)]]  =  0. 

This  only  obeys  the  result  in  general  if  the  constraint  is  linear. 
This  can  be  seen  by  substituting  the  linear  projection  function 
(4)  determined  earlier.  ■ 

The  first  part  of  this  theory  has  been  demonstrated  by  the  re¬ 
sults  i  n  Sect!  on  1 1 1  -A .  E  ach  sampi  e  i  n  the  unconstrai  ned  esti  mate 
was  drawn  such  that  the  normalization  constraint  was  obeyed. 
Therefore,  when  a  higher  order  update  strategy  was  used,  the 
mean  did  not  change.®  The  second  part  of  this  theory  is  illus¬ 
trated  in  Fig.  8(a).  One  thousand  unnormalized  samples  were 
drawn  at  random,  and  (12)  was  applied  to  each  one.  As  can  be 
seen,  the  points  do  not  lie  on  the  circle.  Rather  they  lie  on  a 
parabola  (dueto  the  quadratic  term  in  (10)).  N  ear  the  mean  term 
( 1,0),  the  transformed  poi  nts  appear  to  I  i  e  cl  ose  to  the  uni t  ci  rcl  e. 
However,  Fig.  8(b)  showsaclosein  view.  Although  many  of  the 
samples  lie  on  the  unit  circle,  many  lie  within  it  as  well. 

By  contrast,  the  projection  method  always  guarantees  that  the 
Type  I  constraint  is  applied.  This  follows  trivially  from  the  def¬ 
inition  of  the  projection  operator  in  (2).  However,  because  of 

®The  covariance  was  reduced  slightly  because  the  UT  only  approximates 
terms  up  to  the  fourth  order  and  does  not  capture  them  completely. 
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Jensen’s  inequality,  the  mean  of  this  distribution  does  not  lie 
upon  the  unit  circle  but  within  it. 

V.  New  Approach  for  Applying  Nonlinear 
Equality  Constraints 

The  analysis  in  the  previous  section  shows  that  there  are  sig¬ 
nificant  problems  with  existing  algorithms  for  applying  non¬ 
linear  constraints:  The  projection  method  enforces  the  Type  I 
constraint,  but  thisdoes  not  guarantee  that  theTypell  constraint 
will  be  satisfied.  The  pseudo-observation  method  does  not  guar¬ 
antee  that  either  type  of  constraint  will  be  satisfied. 

We  propose  that  the  nonlinear  equality  constraints  should  be 
applied  in  a  two-step  process  of  applying  the  Type  I  constraint 
followed  by  theTypell  constraint. 

A.  Applying  the  Type  I  Constraint 

Suppose  the  unconstrained  estimate  has  mean  x*{k\k)  and 
covariance  P*{k\k).  The  first  step  is  to  apply  the  Type  I  con- 
strai  nt.  T  hi  s  constrai  ns  the  state  di  strl  buti  on  and,  thus,  causes  the 
covariance  to  decrease.  As  the  analysis  in  the  previous  section 
has  shown,  the  pseudo-observation  method  provides  a  poor  ap¬ 
proximation  to  this  constraint.  Therefore,  the  projection  method 
should  be  applied.  Letxf(A:)  betheTypel  constrained  estimate. 
Therefore 


xf(fc)  =  p[x*(fc)]. 

The  mean  of  the  Type  I  constrained  estimate  xf(fc|A:)  is  cal¬ 
culated  from 


±\k\k)  =  E[p[x*(A:)]]. 

The  covariance  pf(fc|A:)  is  calculated  in  a  similar  manner. 

It  is  important  to  note  that  the  projection  operator  can  contain 
multiple  degrees  of  freedom,  such  as  D  in  (4).  The  degree  of 
freedom  must  be  chosen  to  minimize  some  suitable  measure  of 
uncertainty,  such  as  the  trace  or  determinant  of  P'i'(A;|fc). 

B.  Applying  the  Type  II  Constraint 

According  to  Theorem  1,  xf(fc|A:)  will  not,  in  general,  obey 
the  Type  11  constraint.  Therefore,  we  apply  the  projection  oper¬ 
ator  to  the  Type  I  constrained  estimate  to  generate  a  new  con¬ 
strained  mean 


±(k\k)  =  p[x'l'(A;|fc)]. 

However,  ±^k\k)  is  the  constrained  estimate  with  the  min¬ 
imum  mean  squared  error.  By  moving  the  estimate  to  a  different 
value,  the  filter  ceases  to  propagate  the  conditional  mean,  and 
the  covariance  must  increase  so  that 

Pik\k)  =  P\k\k)  +  {±{k\k)  -  x^'(A:|fc)) 

X  (y(k\k)  —  xf(A:|fc))^  .  (13) 
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F  i  g.  9.  T  he  lc7  contours  for  esti  mates  constrai  ned  usi  ng  the  two-step  algorithm. 
The  estimates  are  shown  as  solid  ellipses  with  centers  at  o.  The  unconstrained 
estimates  are  the  dashed  ellipses  with  centers  at  +,  and  the  unit  circle  is  the 
dotted  line. 


A  gain,  p[-]  may  contain  free  parameters,  and  these  need  to  be 
chosen  so  that  the  uncertainty  in  P{k\k)  is  minimized. 

The  effect  of  this  two-step  process  is  shown  in  Fig.  9  using 
the  projection  function  (11),  and  the  cost  metric  was  to  minimize 

||x(fc|A;)  —  x'l'(fc|A;)||. 

As  the  figure  shows,  the  mean  of  the  estimate  obeys  the  nor¬ 
malization  constraint.  The  covariances  are  non-singular  and  are 
qualitatively  similar  in  shape  to  the  unconstrained  estimates. 
Fig.  10  shows  the  effect  of  the  two-step  algorithm  on  1000 
M  onte  Carlo  samples:  The  Type  I  constraint  projects  the  points 
to  lie  on  a  circular  arc,  and  the  Type  II  constraint  offsets  the 
points  such  that  the  mean  lies  on  the  unit  circle. 

We  now  illustrate  this  algorithm  on  a  platform  tracking 
example. 


VI.  Platform  Tracking  Example 

We  consider  a  vision-based  head  tracking  system.  The  system 
is  composed  of  a  head-mounted  camera  that  moves  through  an 
environment  that  is  populated  by  a  set  of  landmarks  in  known 
locations.  The  position,  orientation,  and  velocity  of  the  camera 
is  to  be  estimated  based  on  its  observations  of  those  landmarks 
[23].  Because  the  camera  is  head  mounted,  it  can  undergo  sig¬ 
nificant  linear  and  angular  accelerations. 

0  rientation  is  part  of  the  SO (3)  group  and  can  be  represented 
by  three  parameters  using  a  number  of  different  representations, 
such  as  Euler  angles  [24j  and  the  exponential  map  [25].  How¬ 
ever,  all  minimal  parameter! zations  possess  singularities  that 
can  lead  to  filter  instability.  The  unit  quaternion  is  the  lowest 
dimensional  nonsingular  attitude  representation  [13]  and  has 
been  widely  used  in  augmented  reality  applications  [23],  [26]. 
However,  it  introduces  the  nonlinear  equality  constraint  that 
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where 
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At  At^ 

a  =  —ujx{k)  +  —ax{k) 

At  Afi 

b  +  -^OLyik) 

At  At^ 

c=  -Y^zik)  +  —a^{k) 

d  =  \/  cP-  }p-  . 


Let  R(A:)  be  the  rotation  matrix  encoded  by  q(A:)  and  K  be 
the  intrinsic  matrix.  Let  be  the  location  of  the  ith  beacon  in 
3-D.  The  camera  observes  the  {x,y)  pixei  position  of  on  the 
imaging  piane.  This  is  given  by 


z(fc)  = 


+  w{k) 
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Fig.  10.  Effect  of  the  two-step  method  on  the  unnormalized  distribution.  The 
unitcircle  is  drawn  with  the  dashed  line,  and  the  M  onte  Carlo  samples  are  drawn 
as  points. 


the  norm  of  the  quaternion  states  must  be  one.  Hybrid  param- 
eterizations,  such  as  the  muitiplicative  extended  Kalman  filter 
[  13],  have  been  devel oped  that  store  errors  i  n  a  three-component 
vector  and  global  orientation  as  a  unit  quaternion.  Periodically, 
the  error  vector  is  reset,  and  its  effects  are  multiplied  into  the 
global  quaternion.  A  Ithough  these  methods  automatically  guar¬ 
antee  that  the  unit  quaternion  is  preserved,  they  do  so  by  repa¬ 
rameterizing  the  problem  and,  as  explained  earlier,  do  not  fall 
within  the  scope  of  this  paper. 

We  use  the  quaternion  representation  from  Davison  [23],  and 
the  state  space  is 


x{k)  = 


-r{k)- 
q{k) 
v{k) 
.u}{k) . 


where  v{k)  =  [xyzY  is  the  position,  q(A:)  =  [qxqyqzQwY  is 
the  orientation  (represented  as  a  unit  quaternion  [27]),  v(fc)  = 
[vxVyVzY  is  the  velocity,  and  a;(A;)  =  is  the  angular 

velocity. 

The  discrete-time  process  model  is 


where 


kx 

hy 

hz 


iKR(A:)(yi  -  v{k)) 


and  w{k)  is  the  additive  observation  noise. 

The  camera  is  a  UniBrain  Fire-1  camera  with  an  85°  field 
of  view  lens  and  a  frame  rate  of  30  frames/s.  w(fc)  is  a  diag¬ 
onal  whose  non-zero  elements  have  a  standard  deviation  of  0.2 
pixels.  Assuming  that  lens  distortion  correction  has  already  been 
applied 


K  = 


883.31 

0 

0 


1.27586  366.132 
883.441  211.048 

0  1 


Because  the  camera  is  handheld,  it  can  experience  extremely 
rapid  and  unmodelled  motions.  Davison  addresses  these  by 
using  very  large  values  for  the  process  noise  standard  devia¬ 
tions:  2  ms“2  for  linear  acceleration  and  4  rads“^  for  angular 
acceleration.  The  initial  covariance  is  diagonal.  The  standard 
deviations  on  the  positions  are  0.1  m,  the  standard  deviations 
on  each  quaternion  value  is  lO"^,  the  linear  velocity  2  ms“L 
and  angular  velocity  3.16  rads“L 
The  construction  of  R(A:)  assumes  that  the  quaternion  is  nor¬ 
malized.  If  the  quaternion  is  not  normalized,  R(fc)  does  not  cor¬ 
respond  to  a  rotation  matrix  and,  in  general,  may  not  have  real 
eigenvalues  [13].  Therefore,  normalization  is  extremely  impor¬ 
tant.  The  constraint  and  projection  functions  are 


x{k  -F  1)  = 


v{k)  +  At\{k)  + 
D(A;)q(A;) 
v(A;)  +  Ata.(k) 
(jj{k)  +  Ata{k) 


where  a.{k)  is  the  unknown  acceleration,  and  a{k)  is  the  un¬ 
known  angular  acceleration  that  acts  on  the  camera  at  each  time 
step.  D(A;)  is  the  skew  symmetric  matrix  given  by  [10] 


c[x(fc),A;]  =  ||q(A;)||  -  1 
p[x(A:)]  =  q(A:)/||q(A:)||.  (14) 

To  test  the  performance  of  the  filters,  2000  Monte  Carlo  runs 
were  performed,  and  the  average  normalized  state  error  was  cal¬ 
culated.  The  normalized  state  error  is  defined  as 

q(k)  =  (x(fc)  -  x{k\k))'^p-'^{k\k){x{k)  -  x(k\k)). 


D(A;)  =  I  cos  d  + 


0  c 
-c  0 
b  —a 
—a  —b 


—b  a 
a  b 
0  c 
—c  0 


sind 


U  si  ng  the  normal  i zed  state  error  provi  des  a  more  preci  se  mea¬ 
sure  of  consistency  than  looking  at  state  estimates  and  covari¬ 
ances  individually  because  this  metric  takes  into  account  the 
cross  correlations  between  the  states  as  well.  The  mean  value 
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TABLE  I 

Mean  Normalized  State  Errors  for  all  Filters  and  the  Decomposition  for  Individual  Subsets  of  States.  The  First  Line  Gives  the  Desired 
Values.  The  First  Five  Columns  Give  the  Normalized  State  Error  for  the  Entire  State  Vector  and  for  the  Position,  Quaternion, 
Velocity,  and  Angular  Velocity  States.  These  Were  Calculated  When  the  Maximum  Average  Normalized  State  Error  is  20. 

The  Final  Column  Lists  the  Normalized  State  Error  When  the  Angle  State  Error  Threshold  was  Raised  to  100 


Algorithm 

E[g(fc)] 

E[g,(fc)] 

Ek(fc)] 

E[5„(fc)] 

E[g..(fc)] 

E[g(fc)] 

- 

13 

3 

4 

3 

3 

13 

displOffNoP 

displOff 

pseudoLinOff 

pseudoUnsOff 

SCKF 

projLin 

projUnsOff 

3.2  X  10** 
18 

2.7  X  10^^ 
3.5  X  10^ 
29 

1.1  X  IQi^ 
13 

~TaxW~ 

3.1 

2.6  X  lO'^ 
13 

4.2 

1.2  X  10^ 
2.9 

4.4  X  10*^ 
3.7 

1.9  X  10“ 
3.3  X  102 
6.9 

9  X  10i° 
3.7 

5.6  X  10^ 
2.8 

4.8  X  10^ 
3.6 

2.9 

6.3  X  10^ 
2.8 

6.5  X  10"^ 
2.9 

1  X  10^ 

3.1 

3.1 

1.9  X  10^ 
2.8 

3.5  X  10** 
24 

3.3  X  10“ 

1.4  X  10^ 
54 

1.3  X  10“ 
15 

of  q{k)  should  be  the  same  as  the  dimensions  of  the  state.  In 
this  case,  the  vaiue  is  13. 

Seven  filters  were  impiemented  where  the  only  difference  be¬ 
tween  them  was  the  aigorithm  used  to  enact  the  normalization 
step.  The  algorithms  were  chosen  to  compare  those  found  in  the 
literature  with  our  two-step  method  and  to  expi ore  the  effects  of 
linearization  and  higher  order  transformations. 

•  displOffNoP:  This  is  the  simpiest  method.  The  Type  II 
constraint  is  satisfied  simply  by  displacing  the  mean  to  es- 
nure  that  the  Type  1 1  constrai  nt  only  i s  appi  i ed.  T he  covari¬ 
ance  is  unchanged.  Therefore 

x(k\k)  =  p[x*(A;|fc)] 

P(k\k)  =  P*{k\k). 

This  approach  was  used  by  Davison  [23]. 

•  displOff:  Like  above,  theType  II  constraint  is  satisfied  by 
dispiacing  the  mean.  However,  the  covariance  is  updated 
to  reflect  the  fact  that  the  mean  has  been  displaced 

x(fc|A;)  =  p[x*(A;|fc)] 

P{k\k)  =  P*{k\k)  +  (x(A:|fc)  -  x*{k\k)) 

X  (x(A;|fc)  —  x*(k\k))^. 

This  illustrates  the  effect  of  the  second  step  in  the  two-step 
process. 

•  psaidoLinOff:  Apply  (5)  and  (6).  Because  the  estimate 
does  not  obey  the  Type  II  constraint,  the  displOff  algo¬ 
rithm  was  used  to  reposition  the  estimate.  We  did  not  use 
thelEKF  approach  from  Section  II  l-A  because  the  filter  al¬ 
ways  diverged. 

•  pseudoUnsOff:  Apply  (5)  and  (6)  but  use  the  unscented 
transformation.  Again,  because  the  estimate  does  not  obey 
the  Type  II  constraint,  the  displacement  offset  algorithm 
was  used  to  reposition  the  estimate. 

•  SCKF:  Apply  the  smoothly  constrained  Kalman  filter  [9] 
for  tight  constraints  using  4, threshold  =  100. 

•  projLin:  Apply  linearized  forms  of  (7)  and  (8). 

•  projUnsOff:  This  is  the  two-step  algorithm:  The  projec¬ 
tion  method  is  applied  using  the  LIT,  and  the  estimate  is 
displaced  so  that  theType  II  constraint  is  satisfied. 

Two  important  limitation  were  encountered.  First,  if  the  an¬ 
gular  error  becomes  large,  the  filters  would  diverge,  irrespective 
of  the  normalization  algorithm  used.  We  believe  this  is  a  fun¬ 
damental  limitation  of  using  the  Kalman  filter  with  a  linear  up¬ 
date  rule  and  a  simple  state  representation  with  a  single  mean 


and  covariance.  We  found  that  a  simple  method  for  indicating 
if  the  angular  bounds  were  exceeded  was  to  test  the  normal¬ 
ized  state  error  of  all  filters.  If  all  of  them  exceeded  a  minimum 
val  ue,  it  was  assumed  that  the  angular  error  constrai  nts  were  ex¬ 
ceeded  and  the  run  was  excluded  from  the  results.  Two  thresh¬ 
olds — 20  and  100 — were  used,  and  the  results  are  discussed 
below.  Second,  q{k)  exhibited  numerical  instabilities  for  the 
pseudoLinOff  and  projLin  algorithms.  These  algorithms  pro¬ 
duced  (nearly)  singular  P{k\k)  matrices  with  extremely  large 
condition  numbers.  An  example  of  the  problem  was  discussed 
in  Section  1 1  l-A :  The  covariance  matrix  issingular  in  a  direction 
in  which  a  non-zero  error  must  occur.  To  generate  finite  results 
for  comparison  purposes,  we  used  a  modified  form  the  normal¬ 
ized  state  error  for  these  filters  of  the  form 

q{k)  ={x{k)  -  x{k\k))^ {P{k\k)  + 

X  (x{k)  —  :k{k\k)) 

where  ^  =  10“'^  is  a  small  constant,  and  N  is  a  matrix  with  ones 
on  the  diagonals  for  states  4-7  (corresponding  to  the  quater¬ 
nions)  and  zeros  elsewhere. 

The  performance  of  the  filters  was  calculated  over  2000 
M  onte  Carlo  runs,  and  the  means  of  the  normalised  state  errors, 
calculated  across  all  runs,  are  presented  in  Table  I.  The  two 
linearized  solutions  (pseudoLinOff  and  projLin)  perform  ex¬ 
tremely  poorly.  The  pseudoUnsOff  filter  performed  somewhat 
better,  but  its  results  were  still  very  poor.  This  illustrates  the 
inadequacies  of  the  the  pseudo-observation  equations,  even 
when  higher  order  prediction  methods  are  used.  The  SCKF 
fares  moderately  well,  but  at  no  point  is  the  quaternion  estimate 
truly  normalized — its  lower  and  upper  bound  lies  between  0.98 
and  1  - 10“®.  The  two  best  behaving  algorithms  were  displOff 
(the  second  step  in  the  two-step  algorithm)  and  the  two-step 
algorithm  itself.  The  two-step  algorithm  is  the  only  algorithm 
that  is  consistent.  To  perform  a  more  finegrained  analysis  of  the 
performance  of  the  filters,  we  calculated  the  normalized  state 
errors  for  the  position,  quaternion,  linear,  and  angular  velocity 
subvectors  only.  The  results,  also  shown  in  Table  I,  indicate 
that  the  effects  of  the  constraint  algorithms  are  most  strongly 
felt  by  the  quaternion  estimates  themselves.  These  are  strongly 
tied  to  the  estimates  of  position  and  angular  velocity.  Linear 
velocity  is  affected  less  by  the  normalization  rule. 

Because  the  platform  starts  from  rest  and  is  buffeted  by 
random  noise,  the  variance  in  the  angular  and  linear  velocities 
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Fig.  11.  Natural  logarithm  of  the  normalized  state  error  for  the  different  filters. 
pseudoLinOff  and  projLin  have  the  largest  means,  with  values  about  30.  dis- 

pOffNoP  lies  in  the  middle  with  a  mean  of  about  15.  dispOff,  pseudoU  nsOff, 
SCKF,  and  projUnsOff  all  have  means  of  around  2.5. 


increase  over  time.  To  study  if  these  have  an  effect  on  the 
filters,  Fig.  11  plots  the  mean  normalized  trajectories  over  the 
duration  of  the  run.  The  results  show  that  for  most  filters,  the 
mean  squared  error  profiles  remain  about  the  same  through  the 
entire  run.  However,  both  the  projLin  and  SCKF  filters  show 
step  changes:  the  former  at  about  10s  and  the  latter  at  about  6s. 
We  believe  that  these  are  caused  by  repeated  divergence  of  the 
filters  at  these  timesteps  because,  at  large  linear  and  angular 
velocities,  the  effects  of  errors  become  more  significant. 

A  s  explained  above,  we  used  a  threshold  to  determine  if  a  run 
was  not  valid.  The  last  column  of  the  table  shows  the  results  of 
2000  M  onte  Carlo  runs  when  the  threshold  was  increased  from 
20  to  100,  allowing  runs  with  larger  angular  errors  through.  In 
this  situation,  the  normalized  errors  in  all  filters  increase,  and 
all  filters  are  Inconsistent.  However,  the  projUnsOff  filter  is  less 
affected  than  the  other  filters,  indicating  that  it  is  more  robust 
than  the  other  algorithms  i n  the  presence  of  large  angul ar  errors. 

VII.  Conclusion 

This  paper  has  considered  the  problem  of  applying  a  Kalman 
Filter-type  estimator  to  a  system  with  nonlinear  equality  con¬ 
straints  between  its  state  space  variables.  We  have  focused 
on  two  methods — the  pseudo-observation  and  projection 
methods — for  incorporating  these  constraints,  and  we  have  pro¬ 
posed  two  different  definitions  of  constraints.  Type  I  constraints 
define  the  shape  of  the  entire  distribution;  Type  II  constraints 
only  limit  the  conditional  mean  propagated  in  the  filter. 

We  have  argued  that  the  pseudo-observation  method  has  little 
theoretical  rigor  and  enforces  neither  the  Type  I  northeTypell 
constraints.  The  projection  method  ensures  that  theType  I  con¬ 
straint  is  satisfied,  but  theType  1 1  constraint  is  not.  To  overcome 
these  difficulties  and  to  have  a  consistent  estimate  that  obeys  the 
Type  1 1  constraint,  we  have  proposed  a  two-step  process.  In  the 
first  step,  theType  I  constraint  is  applied  using  the  projection 
method  across  the  entire  distribution.  This  causes  the  covari¬ 
ance  to  reduce.  The  second  step  applies  the  projection  method 
again,  this  time  to  the  conditional  mean,  to  ensure  that  the  Type 
II  constraint  is  satisfied.  This  causes  the  covariance  to  increase. 
We  have  also  demonstrated  the  performance  of  the  algorithm  in 
a  platform  tracking  example,  which  is  characterized  by  strong 


linear  and  angular  accelerations  and  is  thus  prone  to  larger  un¬ 
certainties.  The  two-step  method  was  shown  to  be  the  only  al¬ 
gorithm  that  was  cons! stent  with  small  angular  errors,  and  when 
large  angular  errors  are  permitted,  its  performance  degrades  I  ess 
severely. 

T  here  are  a  number  of  avenues  for  future  work.  F  i  rst,  the  oper- 
ation  of  the  projection  function  should  be  studied  in  more  detail. 
The  current  algorithm  only  moves  the  states  that  are  directly  af¬ 
fected  by  the  constraint.  However,  all  the  states  should  be  poten¬ 
tially  affected.  A  similar  behavior  is  observed  when  inequality 
constrai  nts  are  appi  i  ed  [  28] .  T  he  effect  of  usi  ng  the  second-order 
projection  operator  described  in  [12]  could  also  be  investigated. 
Second,  the  effects  of  different  cost  functions  on  the  perfor¬ 
mance  of  filters  should  be  examined.  Third,  as  we  explained  in 
the  introduction,  there  is  a  third  way  to  circumvent  nonlinear 
equality  constraints,  which  is  to  reparameterize  the  system  so 
that  they  are  not  needed.  A  useful  area  of  research  would  be  to 
com  pare  the  performance  of  d  I  ff  erent  track!  ng  al  gori  th  ms  to  see 
if  an  over-constrained  representation  actually  provides  benefits 
or  not.  Finally,  the  example  itself  raises  the  question  as  to  what  is 
the  best  representation  of  orientation  in  head  tracking  problems. 
A  Ithough  many  parameter! zations  of  orientation  have  been  pro¬ 
posed,  few  have  been  directly  compared  against  one  another  to 
ascertain  their  impact  on  estimator  performance. 
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